#pragma once

namespace auto_ros
{
namespace control
{
bool perpend_point2d(const double point_x,const double point_y,const double start_x,const double start_y,
                        const double end_x, const double end_y,double &lambda)
{
    double temp_dem = (end_x-start_x)*(end_x-start_x)+(end_y-start_y)*(end_y-start_y);
    if(temp_dem<0.00001)
    {
        std::cout << "\033[perpend_point2d] failed,start point and end point distance is too small  \033[0m"
                  << std::endl;
        return false;
    }
    double temp_num = (end_x-point_x)*(end_x-start_x)+(end_y-point_y)*(end_y-start_y);
    lambda = temp_num/temp_dem;
    return true;

}
template<class point_2d>
bool perpend_point2d(const point_2d point,const point_2d start_point,const point_2d end_point,double &lambda)
{
    double temp_dem = (end_point.x-start_point.x)*(end_point.x-start_point.x)+(end_point.y-start_point.y)*
                    (end_point.y-start_point.y);
    if(temp_dem<0.00001)
    {
        std::cout << "\033[perpend_point2d] failed,start point and end point distance is too small  \033[0m"
                  << std::endl;
        return false;
    }
    double temp_num = (end_point.x-point.x)*(end_point.x-start_point.x)+
                        (end_point.y-point.y)*(end_point.y-start_point.y);
    lambda = temp_num/temp_dem;
    return true;
}
    
} // namespace control
} // namespace auto_ros